#include "tracking.h"
#include "GPIO.h"
static void GPIO_config(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;                                           // 结构定义
    GPIO_InitStructure.Pin = GPIO_Pin_0 || GPIO_Pin_1 || GPIO_Pin_2 || GPIO_Pin_3; // 指定要初始化的IO,
    GPIO_InitStructure.Mode = GPIO_PullUp;                                         // 指定IO的输入或输出方式,GPIO_PullUp,GPIO_HighZ,GPIO_OUT_OD,GPIO_OUT_PP
    GPIO_Inilize(GPIO_P0, &GPIO_InitStructure);                                    // 初始化
}

void Tracking_init()
{
    GPIO_config();
}

/**
 * @brief 获取循迹小车的状态
 *
 * @param status
 */
void Tracking_get_status(u8 status[])
{
    // 获取小车状态
//status[0] = TRACK_X0;
    status[1] = TRACK_X1;
    status[2] = TRACK_X2;
    status[3] = TRACK_X3;
  //  status[4] = TRACK_X4;
}

/**
 * @brief 获取循迹小车的值
 *
 * @return int
 */
int Tracking_Get_Location()
{
    int count = 0, pos = 0, sum = 0;
    static int last_pos = 0;
    
    if (TRACK_X1 == 1)
    {
        sum += -32;
        count++;
    }
    if (TRACK_X2 == 1)
    {
        sum += 0;
        count++;
    }
    if (TRACK_X3 == 1)
    {
        sum += 32;
        count++;
    }
  
    // 假如都没有检测到物体，返回上一次状态
    if (count == 0)
    {
        return last_pos;
    }
    pos = sum / count;
    last_pos = pos;
    return pos;
}
